import serial
import struct
import time
import os
import numpy as np
import matplotlib.pyplot as plt
from scipy.interpolate import CubicSpline
from mpl_toolkits.mplot3d import Axes3D
from scipy.signal import savgol_filter

class SensorDataCollector:
    def __init__(self, port_1,port_2,baud_rate_1,baud_rate_2,command_hex_1, command_hex_2,expected_length_1, expected_length_2,packet_length_1, packet_length_2,file_path_1,file_path_2):
        self.port_1 = port_1  # 端口名称 com1
        self.port_2 = port_2  # 端口名称 com2
        self.baud_rate_1 = baud_rate_1
        self.baud_rate_2 = baud_rate_2
        self.command_1 = bytes.fromhex(command_hex_1)  # 接受一个十六进制字符串作为输入，并将其转换为对应的字节串
        self.command_2 = bytes.fromhex(command_hex_2)  # 接受一个十六进制字符串作为输入，并将其转换为对应的字节串
        self.expected_length_1 = expected_length_1
        self.expected_length_2 = expected_length_2
        self.packet_length_1 = packet_length_1
        self.packet_length_2 = packet_length_2
        self.file_path_1 = file_path_1
        self.file_path_2 = file_path_2
        self.count = 0

        # 检查并创建路径中的目录
        os.makedirs(os.path.dirname(file_path_1), exist_ok=True)
        os.makedirs(os.path.dirname(file_path_2), exist_ok=True)
        self._prepare_txt_file_1()
        self._prepare_txt_file_2()

    def _prepare_txt_file_1(self):
        # 如果文件存在，清空文件内容
        open(self.file_path_1, 'w').close()
        print(f"文件 {self.file_path_1} 已创建/清空。")

    def _prepare_txt_file_2(self):
        # 如果文件存在，清空文件内容
        open(self.file_path_2, 'w').close()
        print(f"文件 {self.file_path_2} 已创建/清空。")

    def send_command_1(self, ser):
        ser.write(self.command_1)
        print("指令已发送：", self.command_1.hex().upper())

    def send_command_2(self, ser):
        ser.write(self.command_2)
        print("指令已发送：", self.command_2.hex().upper())

    def receive_data_1(self, ser):
        data_1 = ser.read(self.expected_length_1)
        print("接收到的数据：", data_1.hex().upper())
        return data_1

    def receive_data_2(self, ser):
        data = ser.read(self.expected_length_2)
        print("接收到的数据：", data.hex().upper())
        return data

    def process_data_1(self, data_1):
        float_size = 4  # 分割数据
        sensor_data = []  # 存放转换之后的数据
        num_packets = len(data_1) // self.packet_length_1  # 获取数据包的个数

        for i in range(num_packets):
            start = i * self.packet_length_1  # 计算当前数据位于哪里
            frame = data_1[start:start + self.packet_length_1]  # 开始的位置+数据的长度

            # 按照frame的结构提取指定部分的数据
            binary_data = bytes(frame[5:17] + frame[18:30] + frame[31:43] + frame[44:56])

            # 将数据切片成4字节的块
            slices = [binary_data[i:i + float_size] for i in range(0, len(binary_data), float_size)]

            # 将每个块转换为大端序列
            big_endian_data = [slice[::-1] for slice in slices]  # 倒序输出

            # 将大端序列的每个块转换为浮点数
            float_array = [struct.unpack('>f', slice)[0] for slice in big_endian_data]

            sensor_data.extend(float_array)  # 展平成一维数组

        return sensor_data

    def process_data_2(self, data_2):
        sensor_data_2 = []

        for i in range(0, len(data_2), 2):
            # 获取当前三字节的数据
            byte1, byte2= data_2[i], data_2[i + 1]#byte1是高八位，byte2是低八位
            merged_data=(byte1 << 4) | byte2

            # 将得到的数字添加到结果列表中
            sensor_data_2.append(merged_data)

        return sensor_data_2

    def collect_sensor_data_1(self):
        with serial.Serial(self.port_1, self.baud_rate_1, timeout=1) as ser_1:
            self.send_command_1(ser_1)
            data = self.receive_data_1(ser_1)
            processed_data_1 = self.process_data_1(data)
        return processed_data_1

    def collect_sensor_data_2(self):
        with serial.Serial(self.port_2, self.baud_rate_2, timeout=1) as ser_2:
            self.send_command_2(ser_2)
            data = self.receive_data_2(ser_2)
            processed_data_2 = self.process_data_2(data)
        return processed_data_2

    def save_to_txt_1(self, data_1):
        with open(self.file_path_1, 'a') as file:
            file.write(' '.join(map(str, data_1))+'\n' )
        print(f"数据已保存至 {self.file_path_1}")
        self.count += 1

    def save_to_txt_2(self, data_2):
        with open(self.file_path_2, 'a') as file:
            file.write(' '.join(map(str, data_2))+'\n' )
        print(f"数据已保存至 {self.file_path_2}")
        self.count += 1

    def run(self, interval):
        max_length=20
        new_ax=[]
        a1 = []
        a2 = []
        new_ay=[]
        new_bent_date=[]
        sensor_data=[]

        plt.ion()  # 开启一个画图的窗口
        fig, (ax1, ax2,ax3) = plt.subplots(1, 3, figsize=(15, 5))  # 创建两个子图
        while True:  # 持续读取传感器数据
            sensor_data = self.collect_sensor_data_1()
            sensor_data_bentdata=self.collect_sensor_data_2()#四个数据
            sensor_data=(sensor_data[0:48])
            print(sensor_data)
            print(sensor_data_bentdata)
            sensor_data_z= [sensor_data[i] for i in range(2, len(sensor_data), 3)]#提取所有手指z方向的数

            if len(sensor_data_z)==16:
                a_1=sensor_data_z[1]+7000
                a_2=sensor_data_z[2]+6000#手指1
                a_3 = (sensor_data_z[4]+7000)/1.2
                a_6 = sensor_data_z[10] + 3500#手指2
                a_5=sensor_data_z[8]+7500#手指3
                a_7 = sensor_data_z[13] + 7000
                a_8 = sensor_data_z[15]+5000 #手指4

                a1.append(((a_1+a_3+a_5+a_8)/4)-1000)
                a2.append((a_2+a_6+a_7)/3)


                new_ay.append(a1)#原始数据

                sensor_data_bent_data_average=(sensor_data_bentdata[3]+sensor_data_bentdata[2]+sensor_data_bentdata[1])/3
                new_bent_date.append(sensor_data_bent_data_average)

                #如果ay大于40，提取ay数据中的后三十个数据
                if len(new_ay)>max_length:
                    a1 = a1[1:]
                    a2 = a2[1:]
                    new_bent_date=new_bent_date[1:]

                if len(new_ay)<=max_length:
                    new_ax=[num for num in range(len(new_ay))]
                else:
                    new_ax = [num for num in range(len(new_ay) - max_length, len(new_ay))]

                ax1.clear()  # 清除第一个子图
                ax2.clear()  # 清除第二个子图
                #ax3.clear()  # 清除第二个子图
                # 绘制第一个子图（The First Finger）
                ax1.plot(new_ax, a1, color='blue')
                ax1.set_title('The First Finger')
                ax1.set_yticks([number for number in range(-5000, 5000, 1000)])

                ax2.plot(new_ax, a2, color='green')
                ax2.set_title('The Second Finger')
                ax2.set_yticks([number for number in range(-5000, 5000, 1000)])

                #ax3.plot(new_ax, new_bent_date, color='red')
                #ax3.set_title('New_bent_date')
                #ax3.set_yticks([number for number in range(0, 3000, 500)])



                # 调整子图的布局，防止标题和标签重叠
                plt.tight_layout()

                plt.pause(0.8)  # 暂停一段时间，不然画的太快会卡住显示不出来

                time.sleep(interval)

                for i, value in enumerate(sensor_data):
                    print(f"第{i + 1}个数据值的处理结果：{value}")
                # 保存当前数据到TXT
                self.save_to_txt_1(sensor_data)
            else:
                continue

# 示例使用
if __name__ == "__main__":
    collector = SensorDataCollector(
        port_1='COM8',
        port_2='COM4',
        baud_rate_1=1000000,
        baud_rate_2=9600,
        command_hex_1='5A000603035D',
        command_hex_2='FF01020304EF',
        expected_length_1=348,
        expected_length_2=8,
        packet_length_1=58,
        packet_length_2=8,
        file_path_1='E:\\shuju\\cichujue.txt',
        file_path_2 = 'E:\\shuju\\bentdata.txt'
    )
    collector.run(interval=0.1)



